#! /opt/local/bin/python2.6
import numpy as np
import time
import sys
from rwEXR import *


# 3744 23.9
# 5616 35.8


# distortion correction parameters in rad
# 0.207029529537 0.0422547753997


entrancePixel = (3341,5615-2345)
#entrancePixel = (1867.5 + 300,2979.5)
center = (1867.5,2979,5)


f = 8

d = 89+79

T1 = np.matrix([d,-d,0])

R = np.matrix([[0,-1,0],[1,0,0],[0,0,1]])

e1 = np.matrix([-f,d+f,0]) 

e2 = np.matrix([d+f,-f,0])

def Rd(pixel):
    c = (1867.5,2979,5)
    return np.sqrt( (pixel[0] - c[0])*(pixel[0] - c[0]) + (pixel[1] - c[1])*(pixel[1] - c[1]) )


theta = 2*np.arcsin( Rd(entrancePixel) * (23.9/3744.0) / (2.0 * f) )

phi   = np.arctan2( entrancePixel[1] - center[1], entrancePixel[0] - center[0] )

op1 = np.matrix([ np.sin(theta)*np.cos(phi), np.cos(theta), np.sin(theta)*np.sin(phi)])

E = np.matrix( [[0,0,-d],[0,0,-d],[d,-d,0]])

N = np.cross(op1,T1) 

print N

#N = (op1 * E)

N = N.flat

n1 = N[0]
n2 = N[1]
n3 = N[2]

n4 = -n1*(d+f) + n2*f
    
y = np.arange(-100,100,0.1)

z = (-n2*f-n2*y)/(n3)

ru = np.sqrt(z*z+y*y)

rd = 2*f*np.sin(np.arctan2(ru,f)/2)

U = -rd * np.cos(np.arctan2(z,y)) * 3744/23.9
V = rd * np.sin(np.arctan2(z,y)) * 3744/23.9

exrR,exrG,exrB,L,size = readExr("/Network/scratch/Tests/XL/DepthEstimationProject/sourceimages/HDRI/rotationFisheye/exr/vue1.exr")

for (u,v) in zip(U,V):

    exrR[u+1867.5,v+(5616-2979.5)] = 0 
    exrG[u+1867.5,v+(5616-2979.5)] = 1
    exrB[u+1867.5,v+(5616-2979.5)] = 0


createNewOutputImage("epipolar.exr",exrR.T,exrG.T,exrB.T,size)




print e1 * E 










